/**
 * @file prioritize.c
 *
 * COSC 3250 / COEN 4820 Assignment 4
 * Modified By:
 * Andrew Kirkham
 * And
 * Chris Gozdziewski
 */
/* Embedded XINU, Copyright (C) 2010.  All rights reserved. */

#include <kernel.h>
#include <queue.h>

short prioritize(short pid, queue q, ulong key)
{
	int head, tail;
	int index;
	head = queuehead(q);
	tail = queuetail(q);
	index = queuetab[head].next;

	if (isempty(q))
	{
		queuetab[head].next = pid;
		queuetab[tail].prev = pid;
		queuetab[pid].prev = head;
		queuetab[pid].next = tail;
		queuetab[pid].key = key;
	}	
	else
	{
		while ((queuetab[index].key <= key))
		{	
			if (tail == index)
				break;			

			index = queuetab[index].next;
		}
		
		queuetab[queuetab[index].prev].next = pid;
		queuetab[pid].prev = queuetab[index].prev;
		queuetab[index].prev = pid;
		queuetab[pid].next = index;
		queuetab[pid].key = key;
	}

	return pid;
}